//
//  atan2.c
//
//
//  Created by Bram Drenthe on 13-12-13.
//
//

#include <stdio.h>

#include "qmath.h"
#include <math.h>

static const q32_t FOUR_DIV_PI  = 0x145F3;            /*!< Fix16 value of 4/PI */
static const q32_t _FOUR_DIV_PI2 = 0xFFFF9840;        /*!< Fix16 value of -4/PI² */
static const q32_t X4_CORRECTION_COMPONENT = 0x399A;  /*!< Fix16 value of 0.225 */
static const q32_t PI_DIV_4 = 0x0000C90F;             /*!< Fix16 value of PI/4 */

static const q32_t THREE_PI_DIV_4 = 0x00025B2F;       /*!< Fix16 value of 3PI/4 */

q32_t qMul(q32_t a, q32_t b)
{
    long long product = (long long)a * b;
    
    if (product < 0) product--;
    q32_t result = product >> 16;
    result += (product & 0x8000) >> 15;
    return result;
}

q32_t qDiv(q32_t a, q32_t b)
{
    return (q32_t)( (((long long)a)<<(16))/b );
}

q32_t qatan2(q32_t v)
{
    /*if (v < 31040) return (q32_t)(0+(v-0)*65536/65536);
    else if (v < 51504) return (q32_t)(28989+(v-31040)*32768/40118);
    else if (v < 73632) return (q32_t)(43652+(v-51504)*16384/26503);
    else if (v < 100272) return (q32_t)(55280+(v-73632)*8192/18533);
    else if (v < 134768) return (q32_t)(65006+(v-100272)*4096/13684);
    else if (v < 182368) return (q32_t)(73281+(v-134768)*2048/10708);
    else if (v < 252416) return (q32_t)(80334+(v-182368)*1024/8953);
    else if (v < 363232) return (q32_t)(86296+(v-252416)*512/8107);
    else if (v < 554912) return (q32_t)(91245+(v-363232)*256/8120);
    else if (v < 928032) return (q32_t)(95239+(v-554912)*128/9304);
    else if (v < 1788304) return (q32_t)(98323+(v-928032)*64/12897);
    else if (v < 4374032) return (q32_t)(100543+(v-1788304)*32/23859);
    else if (v < 16733600) return (q32_t)(101962+(v-4374032)*16/71288);
    */
    q32_t vo = v;
    if (v < 0) v = -v;
    
    if (v < 15056) v = (0+(v-0)*65536/66674); /* 256 */
    else if (v < 24272) v = (14799+(v-15056)*32768/35755); /* 307 */
    else if (v < 33248) v = (23245+(v-24272)*16384/19548); /* 368 */
    else if (v < 42720) v = (30768+(v-33248)*8192/10942); /* 441 */
    else if (v < 53248) v = (37859+(v-42720)*4096/6288); /* 529 */
    else if (v < 65440) v = (44716+(v-53248)*2048/3722); /* 634 */
    else if (v < 80112) v = (51424+(v-65440)*1024/2281); /* 760 */
    else if (v < 98512) v = (58009+(v-80112)*512/1457); /* 912 */
    else if (v < 122624) v = (64472+(v-98512)*256/979); /* 1094 */
    else if (v < 155984) v = (70777+(v-122624)*128/700); /* 1312 */
    else if (v < 205008) v = (76877+(v-155984)*64/541); /* 1574 */
    else if (v < 283456) v = (82666+(v-205008)*32/465); /* 1888 */
    else if (v < 422832) v = (88053+(v-283456)*16/463); /* 2265 */
    else if (v < 711840) v = (92866+(v-422832)*8/569); /* 2718 */
    else if (v < 1466032) v = (96927+(v-711840)*4/976); /* 3261 */
    else if (v < 4405408) v = (100016+(v-1466032)*2/3010); /* 3913 */
    else if (v < 29368368) v = (101969+(v-4405408)*1/30148); /* 4695 */
    else if (v < 82313216) v = (102797+(v-29368368)*1/557314); /* 5634 */
    else v = 102943;
    return vo > 0 ? v : -v;
}

int main()
{
    float f;
    for (f=-1250;f<1250;f+=0.1f)
    {
        q32_t a = qFromFloat(f);
        float real = atanf(f);
        q32_t b = qatan2(a);
        float ee = floatFromQ(b);
        printf("%.16f;%.16f;%.16f;%.16f\n",f,real,ee,real-ee);
    }
}